#ifndef FOC_CONTROL_H_
#define FOC_CONTROL_H_

#include "esp_log.h"
#include "driver/uart.h"
#include "esp_system.h"
#include "esp_event.h"
#include "esp_log.h"
#include "driver/gpio.h"
#include "cJSON.h"
#include "driver/uart.h"     
#include "driver/ledc.h"
#include "esp_timer.h"
#include "math.h"
#include "as5600.h"
#include "lowpass_filter.h"
#include "pid_controller.h"
#include "currsense.h"

// 限制输出大小
#define _constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define PI 3.1415926535897932384626433832795     // 圆周率
#define _3PI_2 4.71238898038f
#define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f


// 电机使能引脚
#define MOTOR_EN_GPIO        12             // 电机使能引脚
// PWM输出引脚
#define Motor_PWM_A_GPIO           32   
#define Motor_PWM_B_GPIO           33
#define Motor_PWM_C_GPIO           25
// PWM参数
#define PWM_FREQUENCY        30000               // 30kHz
#define PWM_RESOLUTION       LEDC_TIMER_8_BIT    // 8位精度，0~255
#define PWM_DUTY             127                 // 初始占空比50%（255*50%）
// 通道和定时器
#define LEDC_TIMER           LEDC_TIMER_0
#define LEDC_MODE            LEDC_HIGH_SPEED_MODE 



typedef struct
{
    uint8_t PP;                                 // 电极个数
    int8_t DIR;                                 // 方向 
    float voltage_limit_max;                    // 输出电压最大值
    float voltage_power_supply;                 // 母线电压
    float zero_electric_angle;                  // 零点偏移角度
    float Ualpha;                               // 通过帕克逆变换计算的 电压
    float Ubeta;                                // // 通过帕克逆变换计算的 电压
    float Ua;                                   // 通过克莱克逆变换计算的 电压
    float Ub;                                   // 通过克莱克逆变换计算的 电压
    float Uc;                                   // 通过克莱克逆变换计算的 电压
    float dc_a;                                 // 实际输出到三相的电压
    float dc_b;                                 // 实际输出到三相的电压
    float dc_c;                                 // 实际输出到三相的电压
    float shaft_angle;                          // 需要转动的机械角度
    float open_loop_timestamp;                  // 计算时间差
    float motor_target;                         // 目标角度
    esp_timer_handle_t foc_timer;               // 执行定时器
    LowPassFilter lowpassfilter_Iq;             // Iq 滤波器
    PIDController pidcontroller_Iq_curr;        // pid 控制器 电流环 Iq
    AS5600_t as5600_A;                          // A AS5600 
    LowPassFilter lowpassfilter_A;              // 低通滤波通道 A
    PIDController pidcontroller_A;              // pid 控制器 A
    CurrSense currsense_A;                      // A电机的电流传感器值
} FOC_STRUCT;

extern FOC_STRUCT foc_struct;

void Foc_Init(float Volt,uint8_t PP,int8_t DIR,float voltage_limit_max, float lowpassfilter_A);
void FOC_alignSensor();
void Foc_Pid_Control_Init(PIDController *control_A);  // pid 控制器
float velocityOpenloop(float target_velocity);    // 开环速度控制
void postionCloseControl();         // 位置闭环控制
void speedCloseControl();           // 速度闭环控制
void currentCloseControl();         // 单电流环控制
void Foc_Pid_Curr_Control_Init(PIDController *control_curr);  // pid 电流环控制器
void Foc_Run();

#endif